#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from ros_demo.msg import CustomMessage

def talker():
    # 初始化ROS节点
    rospy.init_node('talker_py', anonymous=True)
    
    # 创建发布者
    chatter_pub = rospy.Publisher('chatter', String, queue_size=10)
    cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
    custom_pub = rospy.Publisher('custom_topic', CustomMessage, queue_size=10)
    
    # 设置发布频率
    rate = rospy.Rate(10)  # 10Hz
    
    count = 0
    rospy.loginfo("Python Talker节点启动，开始发布消息...")
    
    while not rospy.is_shutdown():
        # 创建字符串消息
        msg = String()
        msg.data = f"Hello ROS from Python! 消息编号: {count}"
        
        # 发布字符串消息
        chatter_pub.publish(msg)
        
        # 创建速度控制消息
        twist = Twist()
        twist.linear.x = 0.5
        twist.angular.z = 0.3
        cmd_vel_pub.publish(twist)
        
        # 创建自定义消息
        custom_msg = CustomMessage()
        custom_msg.name = "Python Robot"
        custom_msg.id = count
        custom_msg.value = count * 0.1
        custom_msg.position.x = count * 0.1
        custom_msg.position.y = count * 0.2
        custom_msg.position.z = 0.0
        custom_pub.publish(custom_msg)
        
        # 输出日志
        rospy.loginfo(f"发布消息: {msg.data}")
        
        # 按照设定频率休眠
        rate.sleep()
        count += 1

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass
